#include "algoritmos.h"
#include <iostream>

using std::cout;
using std::endl;

Algoritmos::Algoritmos()
{

}

void Algoritmos::initialize(char _nombre, float _ts)
{
    //AL PASAR ALGORITMOS OPTIMIZAR CON HEXA
	step = 9.8*(3.3/(1023*0.206));//206 sensible, menos sensible: 800 (con el jumper a masa es 800)
    offset = 0;
    acel_write_index = 0;
    countSamples = 0;
    for(int i=0; i < 20000; i++)
    {
        aceleracion[i] = 0;
        velocidad[i] = 0;
	    posicion[i] = 0;
             
    }
    
    calibrar = true;
	endMovement = true;
    startSpeed = false;

	porcentajeDeVentana = 2.2;		//1 un valor de step; 2 el doble de step 

	nombre = _nombre;
    ts = _ts;				
   
}

void Algoritmos::procesar(int value)
{

	static int i = 1;
	if(acel_write_index == 20000) acel_write_index = 0;

	if(calibrar)
		if(acel_write_index < CALIBRACION)
		{
			offset += value;
			//cout << nombre << "calibrando " << value << "Index " << acel_write_index <<
				//	" offset: "	<< offset 
					//<< "aca " << (offset / i++)
				//	<< " i" 	<< i							
					//		<< endl;
		}
		else
		{
			offset = offset / CALIBRACION;								
			calibrar = false;
		    acel_write_index = 0;
		    acel_read_index = 0;
		    /*cout << nombre ;
			cout << "calibracion:" << offset ;
			cout << " " << (step * offset);
			cout << " step " << step;
			cout << " ts:" << ts <<  endl;*/
		}

	else
	{	
		double 	valor = (value-offset)*step; 
		//cout <<nombre<< " value: "<< value << " a: " << valor<< " " << " con offset " << offset ;

		//MECHANICAL FILTERING WINDOW (verifica que este en el rango de un +-10%)
		if(IsInWindow(valor) && endMovement ) 			
		{
		    startSpeed = false;
		    reachZero = false;
		}
		else							//si esta en la ventana y no hay movimiento integra
		{
			startMoveIndex  = acel_write_index +1;
		    endMovement = false;
		    startSpeed = true;			//es true cuando empieza algun movimiento
		//	cout << " en ventana" << endl;
		}

		aceleracion[acel_write_index+1] = valor;		
		
		if(startSpeed)
		{
			//cout << aceleracion[index+1] << "\t";
			

			if(!movementEndCheck())
				{
			 		velocidad[acel_write_index+1] = velocidad[acel_write_index] + 
								(( aceleracion[acel_write_index+1] + 
								aceleracion[acel_write_index] ) * ts / 2 );
					
					posicion[acel_write_index+1] = posicion[acel_write_index] 
								+ ((ts * ( velocidad[acel_write_index+1] 
								- velocidad[acel_write_index] )) / 2) 
								+ velocidad[acel_write_index]*ts;
			
//					cout << "vel" <<velocidad[acel_write_index+1] << endl;				
				}
		}
		else 
		{	
			aceleracion[acel_write_index+1] = 0;		//para qeu muestre lindo
			velocidad[acel_write_index+1] = 0;
			posicion[acel_write_index+1] = posicion[acel_write_index];
		}
		
		
		/*cout << " [" << getPosicion() << "] " << '\t';
		if(nombre == 'Y')  		 cout << endl;   			*/
	}
   	
	
   	
  //	cout << nombre << "valor " << value << " Index " << acel_write_index;
  	// 	if(nombre == 'Y')  		 cout << endl;
  		
    acel_write_index++;
    //flush(cout);

}

bool Algoritmos::IsInWindow(double valor)
{

//	cout << step*1.1 << " " << valor ;
	if((valor > -step* porcentajeDeVentana) && (valor < step*  porcentajeDeVentana))
	{
//		cout << "Y" << endl;
		return true;
	}
	else 
	{
		//cout << step*1.1 << " " << valor ;
		//cout << "N" << endl;

		return false;
	}
}	


bool Algoritmos::movementEndCheck()
{
    if(IsInWindow(aceleracion[acel_write_index+1]))
    {
        countSamples++;
        if(countSamples == 5) 
        {
			endMovement = true;
			startSpeed = false;
		}
    }
    else
    {
        countSamples = 0;
    }

    if(countSamples >= 8)
    {
        return true;
    }

    return false;
}

int Algoritmos::getOffset()
{
    return this->offset;
}

double Algoritmos::getStep()
{
    return this->step;
}


bool  Algoritmos::hayVectoresAceleracion()
{
	if((acel_write_index - acel_read_index)>0) return true;
	else return false;

}


double Algoritmos::getPosicion()
{
	//cout << nombre << "pos " << 100*posicion[acel_write_index-1] << "[cm] dice que"<<  endl;
    return posicion[acel_write_index-1] * 100;
}


double Algoritmos::vectorAceleracion()
{
	//cout <<  "Read_index:" <<   acel_read_index << endl;
   	acel_read_index++;
	return aceleracion[acel_read_index-1];
}

double Algoritmos::vectorVelocidad()
{
	//cout << "vel " << velocidad[acel_write_index] << "I:" << acel_write_index << endl;
	return velocidad[acel_read_index-1];
}

double Algoritmos::vectorPosicion()
{
//	cout << "pos " << 100*posicion[acel_write_index] << "[cm] dice que"<<  endl;
	return posicion[acel_read_index-1];
}

